#include "Kinematic.h"

#include <cassert>
#include <cmath>

#include "ai/steering/SteeringOutput.h"
#include "main/MainConstants.h"
#include "math/Utils.h"
#include "physic/PhysicConstants.h"


Kinematic::Kinematic(const Point3& position /*= Point3()*/,
                     const float orientation /*= 0.0f*/,
                     const Vector3& linearVel /*= Vector3()*/)
                     : mPosition(position)
                     , mOrientation(orientation)
                     , mLinearVel(linearVel)
{

}

void integrate(Kinematic * const kinematics,
               const SteeringOutput * const steeringOutputs,
               const size_t numKinematics,
               const float* limitSpeed)
{
    assert(kinematics);
    assert(steeringOutputs);
    assert(numKinematics > 0);

    // PARALLEL_FOR
    for (size_t kinIndex = 0; kinIndex < numKinematics; ++kinIndex) {
        Kinematic& kinematic = kinematics[kinIndex];
        const SteeringOutput& steeringOutput = steeringOutputs[kinIndex];            

        // position += linear velocity * time
        {
            Vector3 updatedLinearVel;
            updatedLinearVel = kinematic.mLinearVel * MS_PER_FRAME_FLOAT;
            kinematic.mPosition += updatedLinearVel;                
        }

        // linear velocity += steering linear velocity * time
        {
            Vector3 updatedLinearVel;
            updatedLinearVel = steeringOutput.mLinearVel * MS_PER_FRAME_FLOAT;
            kinematic.mLinearVel += updatedLinearVel;
        }

        // Begin alignment between entity linear velocity
        // and steering linear velocity
        // First computes the angle in radians that we need to rotate
        // from entity linear velocity to steering linear velocity
        // After that, we should check if our angular velocity 
        // per frame is greater than the remaining angle.
        // If that is the case we rotate using the angle previously obtained,
        // otherwise the rotation based on our angular velocity per frame.
        const float angle = rotationAngle(kinematic.mLinearVel, steeringOutput.mLinearVel);
        if (!areEquals(angle, 0.0f)) {
            const float rotationDirection = (angle < 0) ? -1.0f : 1.0f;
            const float currentFrameAngle = 
                rotationDirection * ANGULAR_VEL_RAD_PER_FRAME;
            if (fabs(angle) < ANGULAR_VEL_RAD_PER_FRAME) {
                rotateAroundYAxis(kinematic.mLinearVel, angle);
            } else {
                rotateAroundYAxis(kinematic.mLinearVel, currentFrameAngle);
            }
        }

        // Clamp linear velocity to max acceleration if necessary
        {
            const float sqrLen = kinematic.mLinearVel.sqrLength();
            const float maxSpeed = (limitSpeed) ? *limitSpeed : MAX_SPEED;
            if (maxSpeed * maxSpeed < sqrLen) {
                kinematic.mLinearVel.setLength(maxSpeed);
            }
        }

        // Orientation always point in linear velocity direction
        kinematic.mOrientation = orientationFromVector(kinematic.mLinearVel);
    }
}